
#ifndef PGRUSBCAM_H
#define PGRUSBCAM_H

// Control definitions, comment out if you want to disable a certain mode
#define SOFTWARE_TRIGGER_CAMERA // Use software trigger instead of hardware trigger
//#define CALLBACK_DEBUG        // Display "frame done" when frame arrives
#define CONFIG_DEBUG		// Display config values before and after change

#define QUEUE_SIZE 5
#include <string.h>

#include <ros/ros.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/CameraInfo.h>
#include <camera_info_manager/camera_info_manager.h>
#include <image_transport/image_transport.h>

#include "FlyCapture2.h"

using namespace FlyCapture2;
using namespace std;

class PgrUsbCam
{
#define MAX_IMAGE_WIDTH  752
#define MAX_IMAGE_HEIGHT 480
#define MAX_IMAGE_STRIDE 3*752  // RGB
#define MAX_IMAGE_SIZE   (MAX_IMAGE_HEIGHT*MAX_IMAGE_STRIDE)

 private:
  std::string name_;
  // ROS image message
  sensor_msgs::Image image_;
  sensor_msgs::CameraInfo info_;

  // Camera info manager
  camera_info_manager::CameraInfoManager info_mgr_;

  // Image Transport publish interface
  image_transport::CameraPublisher pub_;

  // PointGrey camera
  FlyCapture2::Camera cam_;

  bool running_flag;
  bool connected_flag;

  bool configVideoFormat();

  bool configTriggerMode(bool mode);

  void configImageProperty();

  void configCamInfoProperty();

  // Setting camera run time parameters
  void setExposure(bool _auto, bool onoff, unsigned int value);

  void setGain(bool _auto, float value);

  void setShutter(bool _auto, float msec);

  void setBrightness(bool _auto, unsigned int value);

 public:

  PgrUsbCam(PGRGuid* guid, const string& cam_name,
	    ros::NodeHandle nh, image_transport::ImageTransport &it);

  ~PgrUsbCam();

  // Config the cameras before they start
  void configCamParams(int exp, int brt, double gain, double shtr);

  // Config the CameraInfo, which contains calibration information 
  // and is published with the image
  void configCamInfo(const string& frameID, const string& info_url);

  void start();

  void stop();

  void GrabnPublish();

  static void ImageEventCallback(FlyCapture2::Image* pframe, const void* pData);

#ifdef SOFTWARE_TRIGGER_CAMERA
  void SetTimeStamp(ros::Time t);
  bool FireSoftwareTrigger();
#endif
};

#endif
